#include "auto_charge_control.h"

AutoChartgeControl::AutoChartgeControl(ros::NodeHandle nh)
{
	vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 20);
	odom_subscriber_ = nh.subscribe("/odom",1,&AutoChartgeControl::odom_callback,this);
	pose_subscriber_ = nh.subscribe("/initialpose",1,&AutoChartgeControl::pose_callback,this);
	listener = std::make_unique<tf2_ros::TransformListener>(tfbuffer_);
  timer_ = nh.createTimer(ros::Duration(0.1), boost::bind(&AutoChartgeControl::publishTf, this));
  check_times_ = 0;
}

AutoChartgeControl::~AutoChartgeControl()
{

}

void AutoChartgeControl::pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msg)
{
	tf_goal_tansform.translation.x = pose_msg->pose.pose.position.x;
	tf_goal_tansform.translation.y = pose_msg->pose.pose.position.y;
	tf_goal_tansform.translation.z = pose_msg->pose.pose.position.z;
	tf_goal_tansform.rotation.x = pose_msg->pose.pose.orientation.x;
	tf_goal_tansform.rotation.y = pose_msg->pose.pose.orientation.y;
	tf_goal_tansform.rotation.z = pose_msg->pose.pose.orientation.z;
	tf_goal_tansform.rotation.w = pose_msg->pose.pose.orientation.w;
	charger_goal_pose_odom.pose = pose_msg->pose.pose;
  check_times_ = 2;
}

void AutoChartgeControl::odom_callback(const nav_msgs::Odometry &odom_msg)
{
  current_odom_= odom_msg;
}

void AutoChartgeControl::publishTf()
{
  if(check_times_ > 1)
  {
    geometry_msgs::TransformStamped tf_goal_tansformStamped;
    tf_goal_tansformStamped.header.frame_id = "odom";
    tf_goal_tansformStamped.header.stamp = ros::Time::now();
    tf_goal_tansformStamped.child_frame_id = "charger_center";
    tf_goal_tansformStamped.transform = tf_goal_tansform;
    broadcaster_.sendTransform(tf_goal_tansformStamped);
  }
}

bool AutoChartgeControl::moveToPark(geometry_msgs::PoseStamped charger_goal_pose_odom, double Kp_rho, double Kp_alpha, double Kp_beta, 
  double max_vel, double max_angular, double stop_dis)
{
  geometry_msgs::Twist cmd_vel;
  auto x_diff = current_odom_.pose.pose.position.x - charger_goal_pose_odom.pose.position.x;
  auto y_diff = current_odom_.pose.pose.position.y - charger_goal_pose_odom.pose.position.y;
  auto theta = tf2::getYaw(current_odom_.pose.pose.orientation);
  auto theta_goal = tf2::getYaw(charger_goal_pose_odom.pose.orientation);
  ROS_INFO("x_diff = %f,y_diff = %f,theta = %f,theta_goal = %f",x_diff,y_diff,theta,theta_goal);

  auto rho = sqrt(pow(charger_goal_pose_odom.pose.position.x - current_odom_.pose.pose.position.x, 2)
                  + pow(charger_goal_pose_odom.pose.position.y - current_odom_.pose.pose.position.y, 2));
  
  // auto alpha = std::fmod((atan2(y_diff, x_diff) - theta), (2 * M_PI));
  // auto beta = std::fmod((theta_goal - theta - alpha), (2 * M_PI));
  auto alpha = std::fmod((atan2(y_diff, x_diff) - theta + M_PI), (2 * M_PI)) - M_PI;
  auto beta = std::fmod((theta_goal - theta - alpha + M_PI), (2 * M_PI)) - M_PI;
  if(alpha > M_PI){
    alpha = alpha - 2*M_PI;
  }
  if(alpha < -M_PI){
    alpha = alpha + 2*M_PI;
  }
  if(beta > M_PI){
    beta = beta - 2*M_PI;
  }
  if(beta < -M_PI){
    beta = beta + 2*M_PI;
  }
  ROS_INFO("rho = %f,alpha = %f,beta = %f",rho,alpha,beta);

  auto v = Kp_rho * rho;
  auto w = Kp_alpha * alpha - Kp_beta * beta;
  if(alpha < (M_PI / 2) || (alpha > -M_PI / 2))
  {
    v = -v;
    cmd_vel.linear.x = std::max(v, max_vel);
  }
  else{
    cmd_vel.linear.x = std::min(v, -max_vel);
  }
  if(w > 0)
    cmd_vel.angular.z = std::min(w, max_angular);
  else
    cmd_vel.angular.z = std::max(w, -max_angular);
  vel_pub_.publish(cmd_vel);
  ROS_INFO("v = %f,w = %f",cmd_vel.linear.x,cmd_vel.angular.z);

  if(rho < stop_dis)
  { 
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;
    vel_pub_.publish(cmd_vel);
    return true;
  }
  return false;
}

bool AutoChartgeControl::rotateToPark(double max_yaw_diff, double max_angular)
{
  geometry_msgs::Twist cmd_vel;
  geometry_msgs::TransformStamped base2chargeTrans;
  double yaw_diff;
  try
  {
    base2chargeTrans = tfbuffer_.lookupTransform("base_link","charger_center",ros::Time(0));
    yaw_diff = tf2::getYaw(base2chargeTrans.transform.rotation);
    ROS_INFO("yaw_diff = %f",yaw_diff);
  }
  catch(tf2::TransformException& e)
  {
    ROS_INFO("%s",e.what());
  }
  if(abs(yaw_diff) < max_yaw_diff)
  {
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;
    vel_pub_.publish(cmd_vel);
    return true;
  }
  else
  {
    if(yaw_diff > max_yaw_diff)
    {
      cmd_vel.linear.x = 0.0;
      cmd_vel.angular.z = max_angular;
      vel_pub_.publish(cmd_vel);
    }
    else if(yaw_diff < -max_yaw_diff)
    {
      cmd_vel.linear.x = 0.0;
      cmd_vel.angular.z = -max_angular;
      vel_pub_.publish(cmd_vel);
    }
  }
  return false;
}

//主循环
void AutoChartgeControl::spinOnce()
{
  ros::Rate rate(20);
  while(ros::ok())
  {
    if(check_times_ == 2)
    {
      geometry_msgs::TransformStamped base2chargeTrans;
      try
      {
        base2chargeTrans = tfbuffer_.lookupTransform("base_link","charger_center",ros::Time(0));
        double x_diff = base2chargeTrans.transform.translation.x;
        ROS_INFO("x_diff = %f",x_diff);
        if(x_diff < 0)
        {
          check_times_ = 3;
        }
      }
      catch(tf2::TransformException& e)
      {
        ROS_INFO("%s",e.what());
      }
    }
    if(check_times_ == 3)
      if(moveToPark(charger_goal_pose_odom,0.9,2.0,0.3,-0.2,0.5,0.02))
        check_times_ = 4;
    if(check_times_ == 4)
      if(rotateToPark(0.08,0.2))
        check_times_ = 5;
    ros::spinOnce();
    rate.sleep();
  }
}

